#include "AReVi/arClass.h"
#include "AReVi/Lib3D/object3D.h"
#include "AReVi/Utils/abstractStream.h"
using namespace AReVi;

#include "element.h"
#include "event.h"
#include "manager.h"
#include "connector.h"
#include "AReVi/Contrib/arMath.h"


AR_CLASS_NOVOID_DEF(Element, ArObject)

Element::Element(ArCW & arCW) : ArObject(arCW), _local(true) {
    _id_classname = thisRef()->getClass()->getName();
    _id_index = intToStr(thisRef()->getClassID());
    _id_client = Manager::getInstance()->getConnector()->getID();
    updateID();
    
    _linearSpeedMin.x = _linearSpeedMin.y = _linearSpeedMin.z = -numeric_limits<double>::max();
    _linearSpeedMax.x = _linearSpeedMax.y = _linearSpeedMax.z = numeric_limits<double>::max();
    
    _angularSpeedMin.x = _angularSpeedMin.y = _angularSpeedMin.z = -numeric_limits<double>::max();
    _angularSpeedMax.x = _angularSpeedMax.y = _angularSpeedMax.z = numeric_limits<double>::max();

    _actUpdateLocation = Activity::NEW(1.0/30.0);
    _actUpdateLocation->setBehavior(thisRef(), &Element::_updateLocation);
}

Element::~Element() {
}

void Element::updateID() {
    _id = _id_classname + "." + _id_index + "@" + _id_client;
}

void Element::parseID(const StlString & id, StlString & classname, StlString & index, StlString & client) {
    classname = index = client = "";
    int param = 0;
    for (unsigned int i = 0; i < id.size(); i++) {
        if (id[i] == '.') {
            param = 1;
            continue;
        }
        else if (id[i] == '@') {
            param = 2;
            continue;
        }
        switch (param) {
            case 0:
            classname += id[i];
            break;
            case 1:
            index += id[i];
            break;
            case 2:
            client += id[i];
            break;
        }
    }
}

ArRef<Element> Element::createFromEvent(ArRef<Event> evt) {
    if (evt->getType() != "CREATE" && evt->getType() != "INFO") {
        cerr << "internal: Invalid event type for Element creation : " << evt->getType() << endl;
        return Element::nullRef();
    }
    StlString classname, index, client;
    parseID(evt->getEmitterID(), classname, index, client);
    
    ArConstRef<ArClass> myClass = ArClass::find(classname);
    if (! myClass) {
        cerr << "internal: Bad class name " << classname << endl;
        return Element::nullRef();
    }
    
    ArRef<Element> element;
    myClass->newInstance(element);
    element->init();
    
    element->_id_classname = classname;
    element->_id_index = index;
    element->_id_client = client;
    element->updateID();
    
    element->readConfiguration(evt->getIStream());
    element->readLocation(evt->getIStream());
    element->readUpdate(evt->getIStream());

    element->_local = false;
    Manager::getInstance()->getConnector()->addCallback(element->getID(), "UPDATE", element->thisPtr(), &Element::_onUpdate);
    Manager::getInstance()->getConnector()->addCallback(element->getID(), "LOCATION", element->thisPtr(), &Element::_onUpdateLocation);

    return element;
}

/**********************************************
 * Gestion de la vitesse et de l'accélération *
 **********************************************/

void Element::getLinearAcc(double & x, double & y, double & z) {
	x = _linearAcc.x;
	y = _linearAcc.y;
	z = _linearAcc.z;
}
void Element::setLinearAcc(double x, double y, double z) {
	_linearAcc.x = x;
	_linearAcc.y = y;
	_linearAcc.z = z;
	fireUpdateLocation();
}

void Element::getLinearSpeed(double & x, double & y, double & z) {
	x = _linearSpeed.x;
	y = _linearSpeed.y;
	z = _linearSpeed.z;
}
void Element::setLinearSpeed(double x, double y, double z) {
	_linearSpeed.x = x;
	_linearSpeed.y = y;
	_linearSpeed.z = z;
	fireUpdateLocation();
}

void Element::getLinearSpeedLimit(double & xmin, double & ymin, double & zmin, double & xmax, double & ymax, double & zmax) {
	xmin = _linearSpeedMin.x;
	ymin = _linearSpeedMin.y;
	zmin = _linearSpeedMin.z;
	xmax = _linearSpeedMax.x;
	ymax = _linearSpeedMax.y;
	zmax = _linearSpeedMax.z;
}
void Element::setLinearSpeedLimit(double xmin, double ymin, double zmin, double xmax, double ymax, double zmax) {
	_linearSpeedMin.x = xmin;
	_linearSpeedMin.y = ymin;
	_linearSpeedMin.z = zmin;
	_linearSpeedMax.x = xmax;
	_linearSpeedMax.y = ymax;
	_linearSpeedMax.z = zmax;
	fireUpdateLocation();
}


void Element::getAngularAcc(double & x, double & y, double & z) {
	x = _angularAcc.x;
	y = _angularAcc.y;
	z = _angularAcc.z;
}
void Element::setAngularAcc(double x, double y, double z) {
	_angularAcc.x = x;
	_angularAcc.y = y;
	_angularAcc.z = z;
	fireUpdateLocation();
}

void Element::getAngularSpeed(double & x, double & y, double & z) {
	x = _angularSpeed.x;
	y = _angularSpeed.y;
	z = _angularSpeed.z;
}
void Element::setAngularSpeed(double x, double y, double z) {
	_angularSpeed.x = x;
	_angularSpeed.y = y;
	_angularSpeed.z = z;
	fireUpdateLocation();
}

void Element::getAngularSpeedLimit(double & xmin, double & ymin, double & zmin, double & xmax, double & ymax, double & zmax) {
	xmin = _angularSpeedMin.x;
	ymin = _angularSpeedMin.y;
	zmin = _angularSpeedMin.z;
	xmax = _angularSpeedMax.x;
	ymax = _angularSpeedMax.y;
	zmax = _angularSpeedMax.z;
}
void Element::setAngularSpeedLimit(double xmin, double ymin, double zmin, double xmax, double ymax, double zmax) {
	_angularSpeedMin.x = xmin;
	_angularSpeedMin.y = ymin;
	_angularSpeedMin.z = zmin;
	_angularSpeedMax.x = xmax;
	_angularSpeedMax.y = ymax;
	_angularSpeedMax.z = zmax;
	fireUpdateLocation();
}





void Element::fireCreate() {
    ArRef<Event> evt = Event::NEW();
    evt->setEmitter(thisRef());
    evt->setType("CREATE");
    writeConfiguration(evt->getOStream());
    writeLocation(evt->getOStream());
    writeUpdate(evt->getOStream());
    
    Manager::getInstance()->broadcast(evt);
}

void Element::fireInfo() {
    ArRef<Event> evt = Event::NEW();
    evt->setEmitter(thisRef());
    evt->setType("INFO");
    writeConfiguration(evt->getOStream());
    writeLocation(evt->getOStream());
    writeUpdate(evt->getOStream());
    
    Manager::getInstance()->broadcast(evt);
}

void Element::fireUpdate() {
    if (!_local) return;
    ArRef<Event> evt = Event::NEW();
    evt->setEmitter(thisRef());
    evt->setType("UPDATE");
    writeUpdate(evt->getOStream());
    
    Manager::getInstance()->broadcast(evt);
}

void Element::fireDelete() {
    ArRef<Event> evt = Event::NEW();
    evt->setEmitter(thisRef());
    evt->setType("DELETE");
    writeUpdate(evt->getOStream());
    
    Manager::getInstance()->broadcast(evt);
}

void Element::fireUpdateLocation() {
    if (!_local) return;
    ArRef<Event> evt = Event::NEW();
    evt->setEmitter(thisRef());
    evt->setType("LOCATION");
    writeLocation(evt->getOStream());

    Manager::getInstance()->broadcast(evt);
}

void Element::writeLocation(ArRef<AbstractOStream> stream) {
    double x, y, z;
    getPosition(x, y, z);

    stream->writeDouble(x);
    stream->writeDouble(y);
    stream->writeDouble(z);
    extractOrientation(x, y, z);
    stream->writeDouble(x);
    stream->writeDouble(y);
    stream->writeDouble(z);
    stream->writeDouble(_linearSpeed.x);
    stream->writeDouble(_linearSpeed.y);
    stream->writeDouble(_linearSpeed.z);
    stream->writeDouble(_linearSpeedMin.x);
    stream->writeDouble(_linearSpeedMin.y);
    stream->writeDouble(_linearSpeedMin.z);
    stream->writeDouble(_linearSpeedMax.x);
    stream->writeDouble(_linearSpeedMax.y);
    stream->writeDouble(_linearSpeedMax.z);
    stream->writeDouble(_angularSpeed.x);
    stream->writeDouble(_angularSpeed.y);
    stream->writeDouble(_angularSpeed.z);
    stream->writeDouble(_angularSpeedMin.x);
    stream->writeDouble(_angularSpeedMin.y);
    stream->writeDouble(_angularSpeedMin.z);
    stream->writeDouble(_angularSpeedMax.x);
    stream->writeDouble(_angularSpeedMax.y);
    stream->writeDouble(_angularSpeedMax.z);
    stream->writeDouble(_linearAcc.x);
    stream->writeDouble(_linearAcc.y);
    stream->writeDouble(_linearAcc.z);
    stream->writeDouble(_angularAcc.x);
    stream->writeDouble(_angularAcc.y);
    stream->writeDouble(_angularAcc.z);
}

void Element::readLocation(ArRef<AbstractIStream> stream) {
	double x, y, z;
    stream->readDouble(x);
    stream->readDouble(y);
    stream->readDouble(z);
    setPosition(x, y, z);
    stream->readDouble(x);
    stream->readDouble(y);
    stream->readDouble(z);
    setOrientation(x, y, z);
    stream->readDouble(_linearSpeed.x);
    stream->readDouble(_linearSpeed.y);
    stream->readDouble(_linearSpeed.z);
    stream->readDouble(_linearSpeedMin.x);
    stream->readDouble(_linearSpeedMin.y);
    stream->readDouble(_linearSpeedMin.z);
    stream->readDouble(_linearSpeedMax.x);
    stream->readDouble(_linearSpeedMax.y);
    stream->readDouble(_linearSpeedMax.z);
    stream->readDouble(_angularSpeed.x);
    stream->readDouble(_angularSpeed.y);
    stream->readDouble(_angularSpeed.z);
    stream->readDouble(_angularSpeedMin.x);
    stream->readDouble(_angularSpeedMin.y);
    stream->readDouble(_angularSpeedMin.z);
    stream->readDouble(_angularSpeedMax.x);
    stream->readDouble(_angularSpeedMax.y);
    stream->readDouble(_angularSpeedMax.z);
    stream->readDouble(_linearAcc.x);
    stream->readDouble(_linearAcc.y);
    stream->readDouble(_linearAcc.z);
    stream->readDouble(_angularAcc.x);
    stream->readDouble(_angularAcc.y);
    stream->readDouble(_angularAcc.z);
}



void Element::_onUpdate(ArRef<Event> evt) {
    if (evt->getEmitterID() != getID() || evt->getType() != "UPDATE") {
        cerr << "internal: Bad event for update : " << evt->getEmitterID() << "->" << evt->getType() << endl;
        return;
    }
    if (! _local) {
    	readLocation(evt->getIStream());
    	readUpdate(evt->getIStream());
    }
}

void Element::_onUpdateLocation(ArRef<Event> evt) {
    if (evt->getEmitterID() != getID() || evt->getType() != "LOCATION") {
        cerr << "internal: Bad event for updating location : " << evt->getEmitterID() << "->" << evt->getType() << endl;
        return;
    }
    if (_local) return;

    readLocation(evt->getIStream());
	_lastLocationUpdate = Scheduler::get()->getTime();
}

bool Element::_updateLocation(ArRef<Activity>, double) {
	updateLocation();
	return true;
}

void Element::updateLocation() {
	double time = Scheduler::get()->getTime();
	double dt = time - _lastLocationUpdate;
	_lastLocationUpdate = time;
	
	_linearSpeed.x += _linearAcc.x * dt;
	_linearSpeed.y += _linearAcc.y * dt;
	_linearSpeed.z += _linearAcc.z * dt;
	
	_angularSpeed.x += _angularAcc.x * dt;
	_angularSpeed.y += _angularAcc.y * dt;
	_angularSpeed.z += _angularAcc.z * dt;
	
	_linearSpeed.x = clamp(_linearSpeed.x, _linearSpeedMin.x, _linearSpeedMax.x);
	_linearSpeed.y = clamp(_linearSpeed.y, _linearSpeedMin.y, _linearSpeedMax.y);
	_linearSpeed.z = clamp(_linearSpeed.z, _linearSpeedMin.z, _linearSpeedMax.z);
	_angularSpeed.x = clamp(_angularSpeed.x, _angularSpeedMin.x, _angularSpeedMax.x);
	_angularSpeed.y = clamp(_angularSpeed.y, _angularSpeedMin.y, _angularSpeedMax.y);
	_angularSpeed.z = clamp(_angularSpeed.z, _angularSpeedMin.z, _angularSpeedMax.z);
	
	translate(_linearSpeed.x * dt, _linearSpeed.y * dt, _linearSpeed.z * dt);
	roll(_angularSpeed.x * dt);
	pitch(_angularSpeed.y * dt);
	yaw(_angularSpeed.z * dt);
}

//-------- Re-définition des primitives de mouvement ------------------------------------------------
void Element::getPosition(double & pxOut, double & pyOut, double & pzOut) const {
    if (getRepresentation()) getRepresentation()->getPosition(pxOut, pyOut, pzOut);
}
void Element::translate(double dx, double dy, double dz) {
    if (getRepresentation()) getRepresentation()->translate(dx, dy, dz);
}
void Element::setPosition(double px, double py, double pz) {
    if (getRepresentation()) getRepresentation()->setPosition(px, py, pz);
}

void Element::extractOrientation(double & rollOut, double & pitchOut, double & yawOut) const {
    if (getRepresentation()) getRepresentation()->extractOrientation(rollOut, pitchOut, yawOut);
}
void Element::roll(double dRoll) {
    if (getRepresentation()) getRepresentation()->roll(dRoll);
}
void Element::pitch(double dPitch) {
    if (getRepresentation()) getRepresentation()->pitch(dPitch);
}
void Element::yaw(double dYaw) {
    if (getRepresentation()) getRepresentation()->yaw(dYaw);
}
void Element::setOrientation(double roll, double pitch, double yaw) {
    if (getRepresentation()) getRepresentation()->setOrientation(roll, pitch, yaw);
}
